Jupyter environment detected. Enabling Open3D WebVisualizer.
[Open3D INFO] WebRTC GUI backend enabled.
[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.
Даны два облака точек. Найти жёсткое преобразование из одного в другое
Соответвие между начальными и конечными точками не известно
Данные имеют шум
Note
Жёсткое преобразование сохраняет расстояние между точками, т.е. поворот + параллельный перенос
Дано: \(X = \{ \mathbf{x}_1, \mathbf{x}_2, \dots, \mathbf{x}_N \} \subset \mathbb{R}^n\), \(Y = \{ \mathbf{y}_1, \mathbf{y}_2, \dots, \mathbf{y}_N \} \subset \mathbb{R}^n\)
\[ \mathbf{y}_{\pi(i)} = R \mathbf{x}_i + t + \boldsymbol{\epsilon}_i, \quad \forall i = 1, \dots, N, \]
где
\(\boldsymbol{\epsilon}_i \in \mathbb{R}^n\) — вектор шума для точки \(i\), \(\boldsymbol{\epsilon}_i \sim \mathcal{N}(0,\, (\sigma^2, \sigma^2,\sigma^2))\)
\(R \in SO(3)\) — матрица вращения \(R^T R = I\), \(\det(R) = 1\)
\(t \in \mathbb{R}^n\) — вектор смещения,
\(\pi: \{1, \dots, N\} \to \{1, \dots, N\}\) — перестановка индексов.
\[ \underset{R \in SO(3),\,t,\,\pi}{\arg \min}\ \sum_{i=1}^N \| R \mathbf{x}_i + t - \mathbf{y}_{\pi(i)} + \boldsymbol{\epsilon}_i \|^2, \]
\[ Y^T = R X^T P + t \begin{pmatrix} 1 & \dots & 1 \end{pmatrix}_{1 \times N} + E, \]
\[ \underset{R \in SO(3),\,t,\,P}{\arg \min}\ \| R X^T P + t \begin{pmatrix} 1 & \dots & 1 \end{pmatrix}_{1 \times N} - Y^T \|_F^2 \]
\[ \underset{R \in SO(3),\,t,\,P}{\arg \min}\ \| R X^TP + t \begin{pmatrix} 1 & . . . & 1 \end{pmatrix}_{1 \times N} - Y^T \|_F^2 \]
Ключевая сложность — поиск матрицы соответствия.
Целевая функция приобретает дискретную форму — обычные методы оптимизации не применимы
Алгоритм Кабша(-Умеямы) умеет решать успрощенную задачу с известным соответствием
\[ \underset{R \in SO(3),\,t}{\arg \min}\ \| R X^T + t \begin{pmatrix} 1 & \dots & 1 \end{pmatrix}_{1 \times N} - Y^T \|_F^2 \]
Избавимся от матрицы перестановок — будем уменьшать расстояние между облаками.
Изменим целевую функцию
\[ \underset{{R \in SO(3),\ t}}{\arg \min}\ \sum_{i=1}^N \| R \mathbf{x}_i + t - \mathbf{y}_{\sigma(i)} \|^2 \]
Iteartive Closest Point — итеративный метод поиска жёсткого преобразования через сопоставление ближайших точек. В качестве приближенного соответсвия точек используется сопоставление каждой ближайшей к ней (из другого облака). Для полученного соответствия можно использовать алгоритм Кабша
Каждую итерацию выбирается соответствие между ближайшими точками в облаках. Алгоритм Кабша уменьшает расстояние между этими точками. Целевая функция монотонно убывает (невозрастает). Таким образом, ICP приобретает локальную сходимость.
Применимость алгоритма к задаче
Минимум исходной метрики является минимумом так же и для целевой функции ICP
Jupyter environment detected. Enabling Open3D WebVisualizer.
[Open3D INFO] WebRTC GUI backend enabled.
[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.
Точки \(\mathbf{p}\) на сфере — результат нормирования случайного вектора \(\mathbf{v}\) с нормально-распределенными компонентами \(x\), \(y\) и \(z\), с последующим домножением на радиус \(r\) \[ \mathbf{p} = r \cdot \frac{\mathbf{v}}{\|\mathbf{v}\|} \] \[ \quad \mathbf{v} = (x, y, z) \sim \mathcal{N}(0, \sigma^2) \]
Выраженно через воздействие вектора с силой обратно пропорциональной расстоянию до точки приложения


Использование упрощенной метрики ведет к тому, что целевая функция преобретает значительное количество локальных минимумов. Кроме того, сопоставление точек облаков через ближайшие — довольно грубое приближение
Использование алгоритма ограничено
ICP показывает хорошие результаты только при малых углах поворота
Соответствие можно установить на основе свойств, инвариантных к жёстким преобразованиям. Например:
Упорядоченный набор таких свойств записывается как вектор, называемый дескриптором
Каждой точке облака сопоставим точку другого облака, ближайшую по дескрипторам. С появлением соответствия на основе дескрипторов сразу приходит простая и понятная идея - применить желанный алгоритм Кабша. Однако в реальной ситуации можно столкнуться с рядом трудностей
Простое решение - увеличить размер дескриптора! Больше инвариантных свойств — меньше вероятность совпадения ✅
И это решение хоть и кажется забаным, однако некоторые дескрипторы могут насчитывать более 2000 элементов.
Однако, это не всегда помогает. Из-за дубликатов может быть много неверных сопоставлений
Такая ситуация вполне может возникнуть если представить, что шум достаточное возмущение для того, чтобы испортить локальные метрики. В конце концов, можно найти некоторый ближайший дескриптор, однако, не будет ли столь плохое соответствие портить решение?
Что делать с возникающими несоответствиями? Исключать. Это довольно простой и изящный подход можно реализовать с помощью алгоритма RANSAC. В данном случае несоответствие можно вычислить через применение алгоритма Кабша - все пары точек, расстояние между которыми после оптимального поворота выше некоторого порога - исключаются как ложные соответствия. И цикл повторяется.
5.912393093354067

5.912393093354067
